#include "lib/ipa_room_planning.h"

#include <utility>

IpaRoomPlanning::IpaRoomPlanning(rclcpp::Node::SharedPtr &nh, std::shared_ptr<tf2_ros::Buffer> &tf_)
{
    node_handle_ = nh;
    tf_buffer = tf_;
    all_path_pub = node_handle_->create_publisher<nav_msgs::msg::Path>("all_coverage_path", 2);
    section_path_pub = node_handle_->create_publisher<nav_msgs::msg::Path>("section_path", 2);
    test_path_pub = node_handle_->create_publisher<nav_msgs::msg::Path>("test_path", 2);
    room_segmentation_client = std::make_shared<SimpleActionClientD<ClientSegmentation>>(
            node_handle_, std::string("room_segmentation_server"));
    room_sequence_client = std::make_shared<SimpleActionClientD<ClientSequence>>(
            node_handle_, std::string("room_sequence_planning_server"));
    room_exploration_client = std::make_shared<SimpleActionClientD<ClientExploration>>(
            node_handle_, std::string("room_exploration_server"));
    grid_client = node_handle_->create_client<nav2_msgs::srv::GetMapNav2>("/map_server/map");
    controllerD = std::make_shared<PurePursuitControllerD>(node_handle_, tf_buffer);
    ipa_planning_param_ = IpaRoomPlanningParam();
    free_thresh = 0.25;
    occupied_thresh = 0.65;
}

bool IpaRoomPlanning::Moving(double &linear_vel, double &angular_vel)
{
    geometry_msgs::msg::Twist twist;
    controllerD->computeVelocityCommands(geometry_msgs::msg::PoseStamped(), twist);
    linear_vel = twist.linear.x;
    angular_vel = twist.angular.z;
    return !controllerD->Finished();
}

void IpaRoomPlanning::setPlan(std::vector<geometry_msgs::msg::PoseStamped> &path)
{
    std::cout << "plan point num= " << path.size() << std::endl;
    nav_msgs::msg::Path path_;
    path_.header.frame_id = "map";
    path_.header.stamp = node_handle_->now();
    path_.poses = path;
    controllerD->setPlan(path_);
}

bool IpaRoomPlanning::getMap(std::shared_ptr<nav2_msgs::srv::GetMapNav2_Response> &srv_goal)
{
    srv_goal = nullptr;
    auto grid_req_srv = std::make_shared<nav2_msgs::srv::GetMapNav2::Request>();
    std::cout << "Requesting grid!" << std::endl;
    int i = 0;
    while (!grid_client->wait_for_service(std::chrono::seconds(5)))
    {
        i++;
        if (i >= 12) return false;
        std::cout << "Requesting grid  faile ,wait 5s..." << std::endl;
    }
    srv_goal = grid_client->async_send_request(grid_req_srv).get();
    std::cout << "Requesting grid success!" << std::endl;
    return true;
}

bool IpaRoomPlanning::prepareData(std::vector<double> &robot_pos, std::string image_file)
{
    all_room_paths.clear();
    std::vector<double> map_origin(3, 0);
    std::vector<geometry_msgs::msg::Point32> fov_points(4);
    geometry_msgs::msg::Point32 fov_origin;
    ipa_planning_param_.image_file = std::move(image_file);
    fov_points[0].x = -ipa_planning_param_.robot_radius;
    fov_points[0].y = ipa_planning_param_.robot_radius;
    fov_points[1].x = -ipa_planning_param_.robot_radius;
    fov_points[1].y = -ipa_planning_param_.robot_radius;
    fov_points[2].x = ipa_planning_param_.robot_radius;
    fov_points[2].y = -ipa_planning_param_.robot_radius;
    fov_points[3].x = ipa_planning_param_.robot_radius;
    fov_points[3].y = ipa_planning_param_.robot_radius;
    fov_origin.x = 0.;
    fov_origin.y = 0.;
    ipa_planning_param_.fov_points = fov_points;
    ipa_planning_param_.fov_origin = fov_origin;
    ipa_planning_param_.start_pos = robot_pos;
    ipa_planning_param_.robot_pos = robot_pos;
    auto grid_req_srv = std::make_shared<nav2_msgs::srv::GetMapNav2::Request>();
    std::shared_ptr<nav2_msgs::srv::GetMapNav2_Response> srv_goal = nullptr;
    if (use_test_image)
    {
        std::cout << "Could not retrieve grid from map_server" << std::endl;
        if (ipa_planning_param_.image_file == std::string(""))
        {
            std::cout << "no data source !" << std::endl;
            return false;
        }
        try
        {
            std::cout << "data_source: default path image !" << std::endl;
            fromPathGetMat(ipa_planning_param_.image_file);
        }
        catch (...)
        {
            std::cout << "no data source !" << std::endl;
            return false;
        }
    }
    else if (getMap(srv_goal))
    {
        if (srv_goal->data_source == srv_goal->IMAGE_PATH)
        {
            std::cout << "data_source: IMAGE_PATH" << std::endl;
            ipa_planning_param_.image_file = srv_goal->map_path;
            fromPathGetMat(srv_goal->map_path);
        }
        else if (srv_goal->data_source == srv_goal->IMAGE_DATA)
        {
            std::cout << "data_source: IMAGE_DATA" << std::endl;
            cv_bridge::CvImagePtr cv_ptr_obj;
            input_image_msg = srv_goal->input_map;
            cv_ptr_obj = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::MONO8);
            cv_ptr_obj->image.copyTo(origin_map_mat);
            Binarization(origin_map_mat, 250);
        }
        else if (srv_goal->data_source == srv_goal->MAP_DATA)
        {
            std::cout << "data_source: MAP_DATA" << std::endl;
            map_origin[0] = srv_goal->map_origin.position.x;
            map_origin[1] = srv_goal->map_origin.position.y;
            ipa_planning_param_.map_resolution = srv_goal->map_resolution;
            mapToMat(srv_goal->map, origin_map_mat);
            cv::flip(origin_map_mat, origin_map_mat, 0);
            Binarization(origin_map_mat, 250);
            cv_bridge::CvImage cv_image;
            cv_image.encoding = "mono8";
            cv_image.image = origin_map_mat;
            cv_image.toImageMsg(input_image_msg);
        }
        else if (srv_goal->data_source == srv_goal->MAP_DATA_ONLINE)
        {
            online_map = true;
        }
    }
    else
    {
        std::cout << "no data source !" << std::endl;
        return false;
    }
    ipa_planning_param_.map_origin = map_origin;
    done_data = true;
    return true;
}

void IpaRoomPlanning::fromPathGetMat(std::string image_path)
{
    origin_map_mat = cv::imread(image_path, 0);
    Binarization(origin_map_mat, 250);
    std::cout << "map-size: " << origin_map_mat.rows << "x" << origin_map_mat.cols << std::endl;
    cv_bridge::CvImage cv_image;
    cv_image.encoding = "mono8";
    cv_image.image = origin_map_mat;
    cv_image.toImageMsg(input_image_msg);
}

void IpaRoomPlanning::matToMap(nav_msgs::msg::OccupancyGrid &map, const cv::Mat &mat)
{
    map.info.width = mat.cols;
    map.info.height = mat.rows;
    map.data.resize(mat.cols * mat.rows);
    for (int x = 0; x < mat.cols; x++)
    {
        for (int y = 0; y < mat.rows; y++)
        {
            map.data[y * mat.cols + x] = mat.at<int8_t>(y, x) ? 0 : 100;
        }
    }
}

void IpaRoomPlanning::mapToMat(const nav_msgs::msg::OccupancyGrid &map, cv::Mat &mat)
{
    mat = cv::Mat(map.info.height, map.info.width, CV_8U);
    int free_thresh_int = std::rint(free_thresh * 100.0);
    int occupied_thresh_int = std::rint(occupied_thresh * 100.0);
    double map_cell;
    int8_t value;
    for (int x = 0; x < mat.cols; x++)
    {
        for (int y = 0; y < mat.rows; y++)
        {
            map_cell = map.data[y * mat.cols + x];
            if (map_cell < 0 || 100 < map_cell) value = 0; //205
            else if (map_cell <= free_thresh_int) value = 255;
            else if (occupied_thresh_int <= map_cell) value = 0;
            else value = 128;
            mat.at<int8_t>(y, x) = value;
        }
    }
}

void IpaRoomPlanning::Binarization(cv::Mat &map_copy, int threshold, int min_value, int max_value)
{
    for (int y = 0; y < map_copy.rows; y++)
    {
        for (int x = 0; x < map_copy.cols; x++)
        {
            if (map_copy.at<unsigned char>(y, x) >= threshold)
            {
                map_copy.at<unsigned char>(y, x) = max_value;
            } else
            {
                map_copy.at<unsigned char>(y, x) = min_value;
            }
        }
    }
}

void IpaRoomPlanning::Publish()
{
    nav_msgs::msg::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = node_handle_->now();
    path_msg.poses.clear();
    for (auto & all_room_path : all_room_paths)
    {
        path_msg.poses.insert(path_msg.poses.end(), all_room_path.begin(), all_room_path.end());
    }
    std::cout << "all_coverage_path: " << path_msg.poses.size() << std::endl;
    all_path_pub->publish(path_msg);
}

void IpaRoomPlanning::sectionPublish(std::vector<geometry_msgs::msg::PoseStamped> &poses_sub)
{
    nav_msgs::msg::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = node_handle_->now();
    path_msg.poses = poses_sub;
    std::cout << "section_path: " << path_msg.poses.size() << std::endl;
    section_path_pub->publish(path_msg);
}

void IpaRoomPlanning::cutOff(std::vector<geometry_msgs::msg::PoseStamped> &poses, std::vector<int> &ids)
{
    ids.clear();
    ids.push_back(0);
    static double path_eps_sqr = pathEpsSqr();
    for (int i = 0; i < poses.size() - 1; i++)
    {
        if ((pow(poses[i].pose.position.x - poses[i + 1].pose.position.x, 2) +
             pow(poses[i].pose.position.y - poses[i + 1].pose.position.y, 2)) > path_eps_sqr)
        {
            ids.push_back(i);
            ids.push_back(i + 1);
        }
    }
    ids.push_back(poses.size() - 1);
    std::cout << "2* cutOff size= " << ids.size() << std::endl;
}

bool IpaRoomPlanning::calRoomExploration(IpaRoomPlanningParam &exploration_param,
        cv::Mat &exploration, cv::Mat &display_mat)
{
    if (exploration_param.exploration_do_flip)
    {
        cv::flip(exploration, exploration, 0);
    }
    Binarization(exploration, 1);
    sensor_msgs::msg::Image labeling;
    cv_bridge::CvImage cv_image;
    cv_image.encoding = "mono8";
    cv_image.image = exploration;
    cv_image.toImageMsg(labeling);
    ipa_building_msgs::action::RoomExploration_Goal goal;
    goal.input_map = labeling;
    goal.map_resolution = exploration_param.map_resolution;
    goal.map_origin.position.x = exploration_param.map_origin[0];
    goal.map_origin.position.y = exploration_param.map_origin[1];
    goal.map_origin.position.z = exploration_param.map_origin[2];
    goal.robot_radius = exploration_param.robot_radius; // turtlebot, used for sim 0.177, 0.4
    goal.coverage_radius = exploration_param.coverage_radius;
    goal.field_of_view = exploration_param.fov_points;
    goal.field_of_view_origin = exploration_param.fov_origin;
    goal.starting_position.x = exploration_param.start_pos[0];
    goal.starting_position.y = exploration_param.start_pos[1];
    goal.starting_position.theta = exploration_param.start_pos[2];
    goal.planning_mode = exploration_param.planning_mode;
    while (rclcpp::ok() && !room_exploration_client->wait_server(5))
    {
        std::cout << "room_exploration_client Action server not available after waiting, wait 5s..." << std::endl;
    }
    printf("room_exploration_client, sending goal.\n");
    result_exp = nullptr;
    result_exp = room_exploration_client->sendGoal(goal);
    if (result_exp == nullptr)
    {
        printf("Timeout on room segmentation.");
        return false;
    }
    std::cout << "coverage path way point num= " << result_exp->coverage_path_pose_stamped.size() << std::endl;
    all_room_paths.push_back(result_exp->coverage_path_pose_stamped);
    Publish();
    path_eps = result_exp->path_eps;
    path_eps_sqr = path_eps * path_eps;
    printf("room exploration successfully.\n");
    if (exploration_param.exploration_display)
    {
        if (display_mat.cols != colour_segmented_map.cols || display_mat.rows != colour_segmented_map.rows)
        {
            display_mat = colour_segmented_map.clone();
            if (exploration_param.exploration_do_flip)
                cv::flip(display_mat, display_mat, 0);
        }

        const double inverse_map_resolution = 1. / goal.map_resolution;
        for (size_t point = 0; point < result_exp->coverage_path.size(); ++point)
        {
            const cv::Point point1(
                    (result_exp->coverage_path[point].x - exploration_param.map_origin[0]) * inverse_map_resolution,
                    (result_exp->coverage_path[point].y - exploration_param.map_origin[1]) * inverse_map_resolution);
            cv::circle(display_mat, point1, 1, cv::Scalar(200), 1);
            if (point > 0)
            {
                const cv::Point point2((result_exp->coverage_path[point - 1].x - exploration_param.map_origin[0]) *
                                       inverse_map_resolution,
                                       (result_exp->coverage_path[point - 1].y - exploration_param.map_origin[1]) *
                                       inverse_map_resolution);
                cv::line(display_mat, point1, point2, cv::Scalar(128), 1);
            }
        }
        cv::namedWindow("exploration client", 0);
        cv::resizeWindow("exploration client", 800, 800);
        cv::imshow("exploration client", display_mat);
        cv::waitKey(0);
    }
    return true;
}

bool IpaRoomPlanning::calRoomSequence(IpaRoomPlanningParam &sequence_param)
{
    std::cout << "run calRoomSequence" << std::endl;
    while (rclcpp::ok() && !room_sequence_client->wait_server(5))
    {
        std::cout << "room_sequence_client Action server not available after waiting, wait 5s..." << std::endl;
    }
    printf("room_sequence_client, sending goal.\n");
    ipa_building_msgs::action::FindRoomSequenceWithCheckpoints_Goal goal_seq;
    goal_seq.input_map = input_image_msg;
    goal_seq.map_resolution = sequence_param.map_resolution;
    goal_seq.map_origin.position.x = sequence_param.map_origin[0];
    goal_seq.map_origin.position.y = sequence_param.map_origin[1];
    goal_seq.map_origin.position.z = sequence_param.map_origin[2];
    goal_seq.room_information_in_pixel = result_seg->room_information_in_pixel;
    goal_seq.robot_radius = sequence_param.robot_radius;
    cv::Mat map_eroded;
    cv::erode(origin_map_mat, map_eroded, cv::Mat(), cv::Point(-1, -1),
              goal_seq.robot_radius / goal_seq.map_resolution + 2);
    cv::Mat distance_map;
    cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5);
    cv::convertScaleAbs(distance_map, distance_map);    // conversion to 8 bit image
    bool robot_start_coordinate_set = false;
    if (sequence_param.start_pos.size() == 3)
    {
        int u_robot = int((sequence_param.start_pos[0] - goal_seq.map_origin.position.x) / goal_seq.map_resolution);
        int v_robot = int((sequence_param.start_pos[1] - goal_seq.map_origin.position.y) / goal_seq.map_resolution);
        int v1, v2, u1, u2;
        for (int v = 0; v < map_eroded.rows && !robot_start_coordinate_set; ++v)
        {
            for (int u = 0; u < map_eroded.cols; ++u)
            {
                u1 = u_robot + u;
                v1 = v_robot + v;
                u2 = u_robot - u;
                v2 = v_robot - v;
                if (u1 > 0 && v1 > 0 && u1 < map_eroded.cols && v1 < map_eroded.rows)
                {
                    if (map_eroded.at<uchar>(v1, u1) != 0 && distance_map.at<uchar>(v1, u1) > 7)
                    {
                        goal_seq.robot_start_coordinate.position.x =
                                u1 * goal_seq.map_resolution + goal_seq.map_origin.position.x;
                        goal_seq.robot_start_coordinate.position.y =
                                v1 * goal_seq.map_resolution + goal_seq.map_origin.position.y;
                        robot_start_coordinate_set = true;
                        break;
                    }
                }
                if (u1 > 0 && v2 > 0 && u1 < map_eroded.cols && v2 < map_eroded.rows)
                {
                    if (map_eroded.at<uchar>(v2, u1) != 0 && distance_map.at<uchar>(v2, u1) > 7)
                    {
                        goal_seq.robot_start_coordinate.position.x =
                                u1 * goal_seq.map_resolution + goal_seq.map_origin.position.x;
                        goal_seq.robot_start_coordinate.position.y =
                                v2 * goal_seq.map_resolution + goal_seq.map_origin.position.y;
                        robot_start_coordinate_set = true;
                        break;
                    }
                }
                if (u2 > 0 && v1 > 0 && u2 < map_eroded.cols && v1 < map_eroded.rows)
                {
                    if (map_eroded.at<uchar>(v1, u2) != 0 && distance_map.at<uchar>(v1, u2) > 7)
                    {
                        goal_seq.robot_start_coordinate.position.x =
                                u2 * goal_seq.map_resolution + goal_seq.map_origin.position.x;
                        goal_seq.robot_start_coordinate.position.y =
                                v1 * goal_seq.map_resolution + goal_seq.map_origin.position.y;
                        robot_start_coordinate_set = true;
                        break;
                    }
                }
                if (u2 > 0 && v2 > 0 && u2 < map_eroded.cols && v2 < map_eroded.rows)
                {
                    if (map_eroded.at<uchar>(v2, u2) != 0 && distance_map.at<uchar>(v2, u2) > 3)
                    {
                        goal_seq.robot_start_coordinate.position.x =
                                u2 * goal_seq.map_resolution + goal_seq.map_origin.position.x;
                        goal_seq.robot_start_coordinate.position.y =
                                v2 * goal_seq.map_resolution + goal_seq.map_origin.position.y;
                        robot_start_coordinate_set = true;
                        break;
                    }
                }
            }
        }
        if (robot_start_coordinate_set)
        {
            std::cout << "use start_pos ~= robot_pos" << std::endl;
        }
    }
    if (!robot_start_coordinate_set)
    {
        std::cout << "try find a start_pos" << std::endl;
        for (int v = 0; v < map_eroded.rows && robot_start_coordinate_set == false; ++v)
            for (int u = 0; u < map_eroded.cols && robot_start_coordinate_set == false; ++u)
                if (map_eroded.at<uchar>(v, u) != 0 && distance_map.at<uchar>(v, u) > 15)
                {
                    goal_seq.robot_start_coordinate.position.x =
                            u * goal_seq.map_resolution + goal_seq.map_origin.position.x;
                    goal_seq.robot_start_coordinate.position.y =
                            v * goal_seq.map_resolution + goal_seq.map_origin.position.y;
                    robot_start_coordinate_set = true;
                }
        std::cout << "find a start_pos" << std::endl;
    }
    result_seq = nullptr;
    result_seq = room_sequence_client->sendGoal(goal_seq);
    if (result_seq == nullptr)
    {
        printf("room sequence: Timeout !\n");
        done_room_sequence = false;
        return false;
    }
    printf("sequence planning successfully!\n");

    if (sequence_param.sequence_display)
    {
        cv_bridge::CvImagePtr cv_ptr_obj;
        cv_ptr_obj = cv_bridge::toCvCopy(result_seq->sequence_map, sensor_msgs::image_encodings::MONO8);
        cv::imshow("sequence client: ", cv_ptr_obj->image);
        cv::waitKey(0);
    }
    optimal_room_sequence.clear();
    display = cv::Mat();
    for (auto & checkpoint : result_seq->checkpoints)
    {
        for (unsigned int room_indice : checkpoint.room_indices)
        {
            optimal_room_sequence.push_back(room_indice);
        }
    }
    done_room_sequence = true;
    return true;
}

bool IpaRoomPlanning::calRoomSegmentation(IpaRoomPlanningParam &segmentation_param)
{
    std::cout << "run calRoomSegmentation" << std::endl;
    while (rclcpp::ok() && !room_segmentation_client->wait_server(5))
    {
        std::cout << "room_segmentation_client Action server not available after waiting, wait 5s..." << std::endl;
    }
    printf("room_segmentation_client, sending goal.\n");
    ipa_building_msgs::action::MapSegmentation::Goal goal_seg;
    goal_seg.data_source = goal_seg.IMAGE_DATA;
    goal_seg.input_map = input_image_msg;
    goal_seg.map_origin.position.x = segmentation_param.map_origin[0];
    goal_seg.map_origin.position.y = segmentation_param.map_origin[1];
    goal_seg.map_origin.position.z = segmentation_param.map_origin[2];
    goal_seg.map_resolution = segmentation_param.map_resolution;
    goal_seg.return_format_in_meter = segmentation_param.return_format_in_meter;
    goal_seg.return_format_in_pixel = segmentation_param.return_format_in_pixel;
    result_seg = nullptr;
    result_seg = room_segmentation_client->sendGoal(goal_seg);
    if (result_seg == nullptr)
    {
        printf("Timeout on room segmentation.\n");
        done_room_segmentation = false;
        return false;
    }
    room_id = 0;
    max_room = result_seg->room_information_in_pixel.size();
    cv_bridge::CvImagePtr cv_ptr_obj;
    cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1);
    colour_segmented_map = cv_ptr_obj->image.clone();
    exploration_map = colour_segmented_map.clone();
    colour_segmented_map.convertTo(colour_segmented_map, CV_8U);
    cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR);
    for (size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i)
    {
        int blue = (rand() % 250) + 1;
        int green = (rand() % 250) + 1;
        int red = (rand() % 250) + 1;
        for (size_t u = 0; u < cv_ptr_obj->image.rows; ++u)
        {
            for (size_t v = 0; v < cv_ptr_obj->image.cols; ++v)
            {
                if (cv_ptr_obj->image.at<int>(u, v) == i)
                {
                    colour_segmented_map.at<cv::Vec3b>(u, v)[0] = blue;
                    colour_segmented_map.at<cv::Vec3b>(u, v)[1] = green;
                    colour_segmented_map.at<cv::Vec3b>(u, v)[2] = red;
                }
            }
        }
    }
    printf("room segmentation successfully!\n");

    if (segmentation_param.segmentation_display)
    {
        cv::Mat display = colour_segmented_map.clone();
        for (size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i)
        {
            cv::Point current_center(result_seg->room_information_in_pixel[i].room_center.x,
                                     result_seg->room_information_in_pixel[i].room_center.y);
            cv::circle(display, current_center, 2, CV_RGB(0, 0, 255), cv::FILLED);
        }
        cv::imshow("segmentation client: ", display);
        cv::waitKey(0);
    }
    done_room_segmentation = true;
    return true;
}

bool IpaRoomPlanning::calVirtualWall(std::vector<std::vector<geometry_msgs::msg::PoseStamped>> *virtual_path)
{
    if (roomID() >= maxRoom())
        return false;
    nav_msgs::msg::Path path;
    std::vector<geometry_msgs::msg::PoseStamped> path_poses, path_poses_tmp, path_stamped_copy;
    geometry_msgs::msg::PoseStamped pose_msg;
    std::vector<std::deque<int>> all_wall_point;
    std::deque<int> tmp_wall_point;
    std::map<int, cv::Point> ori_wall_point, wall_point_copy, wall_point_copy2;
    std::vector<cv::Point2f> path_unorder, path_tmp;
    cv::Point2f pose_point;
    cv::Mat exploration_map_copy = exploration_map.clone();
    pose_msg.header.frame_id = "map";
    path.header = pose_msg.header;
    double map_resolution = ipa_planning_param_.map_resolution;
    int value, index = 0;
    for (size_t v = 0; v < exploration_map_copy.rows; ++v)
        for (size_t u = 0; u < exploration_map_copy.cols; ++u)
            if (exploration_map_copy.at<int>(v, u) == roomID() + 1)
            {
                for (int row_counter = -1; row_counter <= 1; ++row_counter)
                {
                    for (int column_counter = -1; column_counter <= 1; ++column_counter)
                    {
                        value = exploration_map_copy.at<int>(v + row_counter, u + column_counter);
                        if (value != roomID() + 1 && value != 0)
                        {
                            ori_wall_point[index] = cv::Point(v, u);
                            index++;
                            pose_point.x = u * map_resolution + ipa_planning_param_.map_origin[0];
                            pose_point.y = (exploration_map_copy.rows - 1 - v) * map_resolution +
                                           ipa_planning_param_.map_origin[1];
                            path_unorder.push_back(pose_point);
                            /* pose_msg.pose.position.x= pose_point.x;
							pose_msg.pose.position.y=  pose_point.y;
							path_stamped_copy.push_back(pose_msg); */
                        }
                    }
                }
            }
    tmp_wall_point.push_back(0);
    all_wall_point.push_back(tmp_wall_point);
    wall_point_copy = ori_wall_point;
    wall_point_copy.erase(0);
    wall_point_copy2 = wall_point_copy;
    while (rclcpp::ok())
    {
        for (auto it = wall_point_copy.begin(); it != wall_point_copy.end(); it++)
        {
            for (int j = 0; j < all_wall_point.size(); j++)
            {
                int ind;
                if (all_wall_point[j].size() > 1)
                {
                    ind = all_wall_point[j][0];
                    if (abs(ori_wall_point[ind].x - ori_wall_point[it->first].x) < 2 &&
                        abs(ori_wall_point[ind].y - ori_wall_point[it->first].y) < 2)
                    {
                        all_wall_point[j].push_front(it->first);
                        wall_point_copy2.erase(it->first);
                        break;
                    }
                    ind = all_wall_point[j][all_wall_point[j].size() - 1];
                    if (abs(ori_wall_point[ind].x - ori_wall_point[it->first].x) < 2 &&
                        abs(ori_wall_point[ind].y - ori_wall_point[it->first].y) < 2)
                    {
                        all_wall_point[j].push_back(it->first);
                        wall_point_copy2.erase(it->first);
                        break;
                    }
                } else
                {
                    ind = all_wall_point[j][0];
                    if (abs(ori_wall_point[ind].x - ori_wall_point[it->first].x) < 2 &&
                        abs(ori_wall_point[ind].y - ori_wall_point[it->first].y) < 2)
                    {
                        all_wall_point[j].push_back(it->first);
                        wall_point_copy2.erase(it->first);
                        break;
                    }
                }
            }
        }
        if (wall_point_copy2.size() < 1)
            break;
        if (wall_point_copy.size() == wall_point_copy2.size())
        {
            tmp_wall_point.clear();
            tmp_wall_point.push_back(wall_point_copy2.begin()->first);
            all_wall_point.push_back(tmp_wall_point);
            wall_point_copy2.erase(wall_point_copy2.begin()->first);
            if (wall_point_copy2.size() < 1)
                break;
        }
        wall_point_copy = wall_point_copy2;
    }

    tf2::Quaternion quaternion_;
    double line_k, line_b, global_angle, angle1, angle2;
    all_virtual_path.clear();

    /* for(int i=0; i< all_wall_point.size();i++)
    {
        std::cout<< "i=  "<< i << " size= "<< all_wall_point.size()<< std::endl;
        for(int j=0; j< all_wall_point[i].size();j++)
        {
            std::cout<< "j=  "<< j << " size= "<< all_wall_point[i].size()<< std::endl;
            path_poses.push_back(path_stamped_copy[all_wall_point[i][j]]);
        }
    }
    path.poses = path_poses;
    test_path_pub->publish(path); 
    return true; */


    for (int i = 0; i < all_wall_point.size(); i++)
    {
        for (int j = 0; j < all_wall_point[i].size(); j++)
        {
            path_tmp.push_back(path_unorder[all_wall_point[i][j]]);
        }
        cv::Point2f out_point;
        cv::Point2f current_center(result_seg->room_information_in_pixel[Sequence()[roomID()]].room_center.x,
                                   result_seg->room_information_in_pixel[Sequence()[roomID()]].room_center.y);
        LineFit(path_tmp, line_k, line_b);
        GetInlier(line_k, -1, line_b, current_center, out_point);
        global_angle = std::atan2(out_point.y - current_center.y, out_point.x - current_center.x);
        global_angle = NormalizeAngle(global_angle + M_PI_2);
        angle1 = std::atan2(path_tmp[path_tmp.size() - 1].y - path_tmp[0].y,
                            path_tmp[path_tmp.size() - 1].x - path_tmp[0].x);
        angle2 = std::atan2(path_tmp[0].y - path_tmp[path_tmp.size() - 1].y,
                            path_tmp[0].x - path_tmp[path_tmp.size() - 1].x);
        if (NormalizeAngle(global_angle - NormalizeAngle(angle1)) >
            NormalizeAngle(global_angle - NormalizeAngle(angle2)))
        {
            std::reverse(path_tmp.begin(), path_tmp.end());
        }
        downSample(path_tmp, 0.3, 0.2);
        std::vector<double> all_angle;
        for (int i = 0; i < path_tmp.size() - 1; i++)
        {
            global_angle = std::atan2(path_tmp[i + 1].y - path_tmp[i].y, path_tmp[i + 1].x - path_tmp[i].x);
            all_angle.push_back(global_angle);
        }
        all_angle.push_back(NormalizeAngle(all_angle[all_angle.size() - 1] + M_PI_2));
        quaternion_.setRPY(0, 0, all_angle[0]);
        pose_msg.pose.orientation.x = quaternion_.x();
        pose_msg.pose.orientation.y = quaternion_.y();
        pose_msg.pose.orientation.z = quaternion_.z();
        pose_msg.pose.orientation.w = quaternion_.w();
        pose_msg.pose.position.x = path_tmp[0].x;
        pose_msg.pose.position.y = path_tmp[0].y;
        path_poses_tmp.push_back(pose_msg);
        for (int i = 1; i < path_tmp.size(); i++)
        {
            pose_msg.pose.position.x = path_tmp[i].x;
            pose_msg.pose.position.y = path_tmp[i].y;
            if (fabs(all_angle[i] - all_angle[i - 1]) > 0.087266461)  // 0.087266461 = 5deg
            {
                pose_msg.pose.orientation = path_poses_tmp[i - 1].pose.orientation;
                path_poses_tmp.push_back(pose_msg);
            }
            quaternion_.setRPY(0, 0, all_angle[i]);
            pose_msg.pose.orientation.x = quaternion_.x();
            pose_msg.pose.orientation.y = quaternion_.y();
            pose_msg.pose.orientation.z = quaternion_.z();
            pose_msg.pose.orientation.w = quaternion_.w();
            path_poses_tmp.push_back(pose_msg);
        }
        all_virtual_path.push_back(path_poses_tmp);
        path_poses.insert(path_poses.end(), path_poses_tmp.begin(), path_poses_tmp.end());
        path_tmp.clear();
        path_poses_tmp.clear();
    }
    if (virtual_path != nullptr)
        virtual_path = &all_virtual_path;
    path.poses = path_poses;
    std::cout << "test_path: " << path.poses.size() << std::endl;
    test_path_pub->publish(path);
    return true;
}

int IpaRoomPlanning::roomExploration(int room_id_)
{
    std::cout << "maxRoom= " << maxRoom() << " roomID= " << room_id_ << std::endl;
    ipa_planning_param_.start_pos[0] =
            result_seg->room_information_in_pixel[Sequence()[room_id_]].room_center.x;
    ipa_planning_param_.start_pos[1] =
            result_seg->room_information_in_pixel[Sequence()[room_id_]].room_center.y;
    cv::Mat map_copy(exploration_map == Sequence()[room_id_] + 1);
    calRoomExploration(map_copy, display);
    int size = result_exp->coverage_path_pose_stamped.size();
    std::cout << "size= " << size << std::endl;
    return size;
}

void IpaRoomPlanning::downSample(std::vector<cv::Point2f> &path, double dis_between_point, double dis_with_wall)
{
    static double dis_pow = dis_between_point * dis_between_point;
    static double dis_with_wall_sqr = dis_with_wall * dis_with_wall;
    if (dis_with_wall > 1e-3)
    {
        std::vector<cv::Point2f> tmp_path = path;
        cv::Point2f tmp_mid = tmp_path[int(tmp_path.size() * 0.5)];
        cv::Point2f tmp = tmp_path[0];
        for (int i = 0; i < tmp_path.size(); i++)
        {
            if (pow(tmp.x - tmp_path[i].x, 2) + pow(tmp.y - tmp_path[i].y, 2) < dis_with_wall_sqr)
            {
                path.erase(path.begin());
            }
        }
        if (path.size() > 0)
        {
            tmp_path = path;
            tmp = tmp_path[tmp_path.size() - 1];
            for (int i = tmp_path.size() - 1; i >= 0; i--)
            {
                if (pow(tmp.x - tmp_path[i].x, 2) + pow(tmp.y - tmp_path[i].y, 2) < dis_with_wall_sqr)
                {
                    path.erase(std::prev(path.end()));
                }
            }
            if (path.size() <= 0)
            {
                path.push_back(tmp_mid);
                return;
            }
        } else
        {
            path.push_back(tmp_mid);
            return;
        }
    }
    std::vector<cv::Point2f> path_tmp;
    path_tmp.push_back(path[0]);
    for (auto &pose : path)
    {
        if (pow(path_tmp[path_tmp.size() - 1].x - pose.x, 2) +
            pow(path_tmp[path_tmp.size() - 1].y - pose.y, 2) >= dis_pow)
        {
            path_tmp.push_back(pose);
        }
    }
    if (pow(path_tmp[path_tmp.size() - 1].x - path[path.size() - 1].x, 2) +
        pow(path_tmp[path_tmp.size() - 1].y - path[path.size() - 1].y, 2) < dis_pow)
    {
        path_tmp.pop_back();
    }
    path_tmp.push_back(path[path.size() - 1]);
    path = path_tmp;
}

bool IpaRoomPlanning::LineFit(std::vector<cv::Point2f> &points, double &line_k, double &line_b)
{
    int pc_size = (int) points.size();
    if (pc_size > 0)
    {
        double avgX = 0, avgY = 0;    //定义变量
        double lxx = 0, lyy = 0, lxy = 0;
        for (int i = 0; i < pc_size; i++)   //计算X、Y的平均�?
        {
            avgX += points[i].x / pc_size;
            avgY += points[i].y / pc_size;
        }
        for (int i = 0; i < pc_size; i++)   //计算Lxx、Lyy和Lxy
        {
            lxx += (points[i].x - avgX) * (points[i].x - avgX);
            lyy += (points[i].y - avgY) * (points[i].y - avgX);
            lxy += (points[i].x - avgX) * (points[i].y - avgY);
        }
        line_k = lxy / lxx;
        line_b = avgY - lxy * avgX / lxx;
        return true;
    }
    return false;
}

void IpaRoomPlanning::GetInlier(const double &line_a, const double &line_b, const double &line_c,
                                const cv::Point2f &fit_point, cv::Point2f &result_point)
{
    double line_a2 = line_a * line_a;
    double line_b2 = line_b * line_b;
    double line_a2_b2 = line_a2 + line_b2;
    double line_ab = line_a * line_b;
    double line_ac = line_a * line_c;
    double line_bc = line_b * line_c;

    result_point.x = (line_b2 * fit_point.x - line_ab * fit_point.y - line_ac) / line_a2_b2;
    result_point.y = (-line_ab * fit_point.x + line_a2 * fit_point.y - line_bc) / line_a2_b2;

}
